/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef SLAM_MAPPING_MAP_BUILDER_H_
#define SLAM_MAPPING_MAP_BUILDER_H_

#include <memory>

#include "slam/common/thread_pool.h"
#include "slam/mapping/map_builder_interface.h"
#include "slam/mapping/pose_graph.h"
#include "slam/mapping/proto/map_builder_options.pb.h"
#include "slam/sensor/collator_interface.h"

namespace slam {
namespace mapping {

proto::MapBuilderOptions CreateMapBuilderOptions(
    common::LuaParameterDictionary* const parameter_dictionary);

// Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps)
// and a PoseGraph for loop closure.
class MapBuilder : public MapBuilderInterface {
 public:
  explicit MapBuilder(const proto::MapBuilderOptions& options);
  ~MapBuilder() override {}

  MapBuilder(const MapBuilder&) = delete;
  MapBuilder& operator=(const MapBuilder&) = delete;

  int AddTrajectoryBuilder(
      const std::set<SensorId>& expected_sensor_ids,
      const proto::TrajectoryBuilderOptions& trajectory_options,
      LocalSlamResultCallback local_slam_result_callback) override;

  int AddTrajectoryForDeserialization(
      const proto::TrajectoryBuilderOptionsWithSensorIds&
          options_with_sensor_ids_proto) override;

  void FinishTrajectory(int trajectory_id) override;

  std::string SubmapToProto(const SubmapId& submap_id,
                            proto::SubmapQuery::Response* response) override;

  void SerializeState(bool include_unfinished_submaps, const bool cover_old_map,
                      io::ProtoStreamWriterInterface* writer) override;

  bool SerializeStateToFile(bool include_unfinished_submaps,
                            const bool cover_old_map,
                            const std::string& filename) override;

  bool SavePoseGraphAndMap(const std::string& map_name) override;

  std::map<int, int> LoadState(io::ProtoStreamReaderInterface* reader,
                               bool load_frozen_state) override;

  std::map<int, int> LoadStateFromFile(const std::string& filename,
                                       const bool load_frozen_state) override;

  mapping::PoseGraphInterface* pose_graph() override {
    return pose_graph_.get();
  }

  int num_trajectory_builders() const override {
    return trajectory_builders_.size();
  }

  mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder(
      int trajectory_id) const override {
    return trajectory_builders_.at(trajectory_id).get();
  }

  const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>&
  GetAllTrajectoryBuilderOptions() const override {
    return all_trajectory_builder_options_;
  }

  bool IsLocalizationSuccess() const override {
    return pose_graph_->IsLocalizationSuccess();
  }

  float GetMeanMatchScore() const override {
    return pose_graph_->GetMeanMatchScore();
  }

  bool DeleteSubmapById(const int trajectory_id,
                        const int submap_index) override {
    SubmapId submap_id(trajectory_id, submap_index);
    return pose_graph_->DeleteSubmapById(submap_id);
  }

  void SetOnlyUseOdomPredictPoseFlag(const bool flag) {
    trajectory_builders_.back()->SetOnlyUseOdomPredictPoseFlag(flag);
  }

  std::string PgmToLocalizationMap(const std::string& pgm_file_path);

  bool QueryFontLaserDataStatus() {
    return trajectory_builders_.back()->QueryFontLaserDataStatus();
  }

  const PoseGraphData& GetPoseGraphData() const {
    return pose_graph_->GetPoseGraphData();
  }

  PoseGraphData* MutablePoseGraphData() {
    return pose_graph_->MutablePoseGraphData();
  }

  const proto::MapBuilderOptions& GetMapBuilderOptions() const {
    return options_;
  }

  void GenerateGlobalMap(const bool cover_old_map);

  bool GetSubmapSlices(
      const SubmapId& submap_id, const double resolution,
      std::map<SubmapId, io::SubmapSlice>* submap_slices_ptr) const;

  void GlobalMapToPgm(const std::string& map_filename,
                      const double resolution) const;

 private:
  bool DeviceAuthenticate();

 private:
  const proto::MapBuilderOptions options_;
  common::ThreadPool thread_pool_;
  const std::string license_file_;
  bool authenticate_status_;
  std::unique_ptr<PoseGraph> pose_graph_;

  std::unique_ptr<sensor::CollatorInterface> sensor_collator_;
  std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
      trajectory_builders_;
  std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
      all_trajectory_builder_options_;
};

}  // namespace mapping
}  // namespace slam

#endif  // SLAM_MAPPING_MAP_BUILDER_H_
